import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    # Get package directory
    pkg_dir = get_package_share_directory('ego_planner_adapter')
    
    # Launch arguments
    namespace_arg = DeclareLaunchArgument(
        'namespace',
        default_value='',
        description='Robot namespace'
    )
    
    use_sim_time_arg = DeclareLaunchArgument(
        'use_sim_time',
        default_value='false',
        description='Use simulation time'
    )
    
    config_dir_arg = DeclareLaunchArgument(
        'config_dir',
        default_value=os.path.join(pkg_dir, 'config'),
        description='Configuration directory'
    )
    
    # Parameters
    namespace = LaunchConfiguration('namespace')
    use_sim_time = LaunchConfiguration('use_sim_time')
    config_dir = LaunchConfiguration('config_dir')
    
    # Parameter files
    trajectory_tracker_params = PathJoinSubstitution([
        config_dir,
        'trajectory_tracker_params.yaml'
    ])
    
    target_relay_params = PathJoinSubstitution([
        config_dir,
        'target_relay_params.yaml'
    ])
    
    # Nodes
    trajectory_tracker_node = Node(
        package='ego_planner_adapter',
        executable='trajectory_tracker_node',
        name='trajectory_tracker',
        namespace=namespace,
        parameters=[
            trajectory_tracker_params,
            {'use_sim_time': use_sim_time}
        ],
        remappings=[
            ('odometry', 'odometry'),
            ('trajectory', 'ego_planner/trajectory'),
            ('cmd_vel', 'cmd_vel_nav2_result'),
            ('target_altitude', 'target_altitude'),
            ('trajectory_marker', 'trajectory_marker')
        ],
        output='screen'
    )
    
    target_relay_node = Node(
        package='ego_planner_adapter',
        executable='target_relay_node',
        name='target_relay',
        namespace=namespace,
        parameters=[
            target_relay_params,
            {'use_sim_time': use_sim_time}
        ],
        remappings=[
            ('goal_pose', 'goal_pose'),
            ('target_altitude', 'target_altitude_input'),
            ('planning_trigger', 'planning_trigger'),
            ('ego_planner/goal', 'ego_planner/goal'),
            ('ego_planner/trigger', 'ego_planner/trigger'),
            ('goal_marker', 'goal_marker')
        ],
        output='screen'
    )
    
    return LaunchDescription([
        namespace_arg,
        use_sim_time_arg,
        config_dir_arg,
        trajectory_tracker_node,
        target_relay_node
    ])